import numpy as np


def bfgs(f, grad_f, X_cur):
    # 误差
    EPSILON = 0.01
    # 步长
    alpha = 1
    set_X = [X_cur.copy()]
    k = 0
    i = 0
    while True:
        # 2. 初始化H
        if k == 0:
            H = np.identity(X_cur.shape[0])
        dX = grad_f(X_cur)
        # 3.根据梯度判断是否满足误差
        if np.sqrt(np.sum(dX ** 2)) <= EPSILON:
            break
        # 4.下降方向
        H = np.mat(H)
        dX = np.mat(dX).transpose()
        d = -H * dX
        d = np.array(d.transpose())[0]
        # 使用线搜索判断步长
        while f(X_cur + alpha * d) > f(X_cur):
            alpha *= 0.618
        # 5.迭代点
        X_old = X_cur.copy()
        X_cur += alpha * d
        # 打印信息
        print("%d轮迭代：" % i, X_cur)
        i += 1
        k += 1
        # 7. 迭代 H
        if k == X_cur.shape[0] - 1:
            k = 0
        dX1 = np.mat(grad_f(X_cur)).transpose()
        q = np.mat(dX1 - dX)
        p = np.mat(X_cur - X_old).transpose()
        H = H + (1 + (float(q.transpose() * H * q)) / (float(p.transpose() * q))) * (
                (p * p.transpose()) / (float(p.transpose() * q))) - (
                    p * q.transpose() * H + H * q * p.transpose()) / (float(p.transpose() * q))
        set_X.append(X_cur.copy())

    set_X = np.array(set_X)
    return set_X
